function visualize_map_and_trajectory(obstacles, initial_state, controls, dt, map_size, waypoints)
% 可视化地图、障碍物和规划的轨迹
%
% 输入:
%   obstacles - 障碍物列表 [x_min, y_min, x_max, y_max]
%   initial_state - 初始位姿 [x; y; theta]
%   controls - 控制序列 [v; omega]
%   dt - 时间步长
%   map_size - 地图尺寸 [width, height]
%   waypoints - 路径点 [x; y] (可选)

    if nargin < 5
        map_size = [50, 50];
    end
    
    if nargin < 6
        waypoints = [];
    end
    
    figure('Name', '地图与轨迹规划', 'Position', [100, 100, 1000, 900]);
    
    % 预测轨迹
    state = initial_state;
    trajectory = zeros(3, size(controls, 2));
    trajectory(:, 1) = state;
    
    for t = 2:size(controls, 2)
        v = controls(1, t-1);
        omega = controls(2, t-1);
        
        % 简单的运动模型（无噪声）
        state(1) = state(1) + v * cos(state(3)) * dt;
        state(2) = state(2) + v * sin(state(3)) * dt;
        state(3) = state(3) + omega * dt;
        state(3) = wrapToPi(state(3));
        
        trajectory(:, t) = state;
    end
    
    % === 子图1: 地图总览 ===
    subplot(2, 2, 1);
    hold on;
    axis equal;
    grid on;
    xlim([0, map_size(1)]);
    ylim([0, map_size(2)]);
    xlabel('X (m)');
    ylabel('Y (m)');
    title('地图与障碍物布局', 'FontSize', 12, 'FontWeight', 'bold');
    
    % 绘制外墙
    for i = 1:4
        obs = obstacles(i, :);
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
            'FaceColor', [0.3, 0.3, 0.3], 'EdgeColor', 'k', 'LineWidth', 2);
    end
    
    % 绘制内部障碍物
    for i = 5:size(obstacles, 1)
        obs = obstacles(i, :);
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
            'FaceColor', [0.7, 0.3, 0.3], 'EdgeColor', [0.5, 0.1, 0.1], 'LineWidth', 1.5);
    end
    
    % 绘制安全区域
    safety_margin = 3.0;
    rectangle('Position', [safety_margin, safety_margin, ...
        map_size(1)-2*safety_margin, map_size(2)-2*safety_margin], ...
        'EdgeColor', [0.2, 0.8, 0.2], 'LineStyle', '--', 'LineWidth', 1.5);
    
    % 标注
    text(map_size(1)/2, 2, '地图尺寸', 'HorizontalAlignment', 'center', ...
        'FontSize', 10, 'Color', [0.3, 0.3, 0.3]);
    text(safety_margin+1, safety_margin+1, '安全区域', ...
        'FontSize', 9, 'Color', [0.2, 0.8, 0.2]);
    
    % === 子图2: 规划轨迹 ===
    subplot(2, 2, 2);
    hold on;
    axis equal;
    grid on;
    xlim([0, map_size(1)]);
    ylim([0, map_size(2)]);
    xlabel('X (m)');
    ylabel('Y (m)');
    title('规划的机器人轨迹', 'FontSize', 12, 'FontWeight', 'bold');
    
    % 绘制障碍物（简化）
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        if i <= 4
            color = [0.8, 0.8, 0.8];  % 外墙
        else
            color = [0.9, 0.7, 0.7];  % 内部障碍物
        end
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
            'FaceColor', color, 'EdgeColor', 'none');
    end
    
    % 绘制轨迹
    plot(trajectory(1, :), trajectory(2, :), 'b-', 'LineWidth', 2);
    plot(trajectory(1, 1), trajectory(2, 1), 'go', 'MarkerSize', 12, ...
        'MarkerFaceColor', 'g', 'LineWidth', 2);
    plot(trajectory(1, end), trajectory(2, end), 'ro', 'MarkerSize', 12, ...
        'MarkerFaceColor', 'r', 'LineWidth', 2);
    
    % 每隔一段时间绘制机器人方向
    step_interval = max(1, floor(size(trajectory, 2) / 20));
    for t = 1:step_interval:size(trajectory, 2)
        arrow_length = 1.5;
        quiver(trajectory(1, t), trajectory(2, t), ...
            arrow_length * cos(trajectory(3, t)), ...
            arrow_length * sin(trajectory(3, t)), ...
            0, 'Color', [0.3, 0.3, 0.8], 'LineWidth', 1.5, 'MaxHeadSize', 0.5);
    end
    
    % 如果提供了路径点，显示它们
    if ~isempty(waypoints)
        plot(waypoints(1, :), waypoints(2, :), 'mo', 'MarkerSize', 10, ...
            'MarkerFaceColor', 'm', 'LineWidth', 2);
        plot(waypoints(1, :), waypoints(2, :), 'm--', 'LineWidth', 1.5);
        
        % 标注路径点序号
        for i = 1:size(waypoints, 2)
            text(waypoints(1, i), waypoints(2, i) + 1.5, sprintf('WP%d', i), ...
                'HorizontalAlignment', 'center', 'FontSize', 9, ...
                'Color', 'm', 'FontWeight', 'bold');
        end
        
        legend('轨迹', '起点', '终点', '路径点', 'Location', 'best');
    else
        legend('轨迹', '起点', '终点', 'Location', 'best');
    end
    
    % === 子图3: 速度曲线 ===
    subplot(2, 2, 3);
    hold on;
    grid on;
    time = (0:size(controls, 2)-1) * dt;
    plot(time, controls(1, :), 'b-', 'LineWidth', 1.5);
    xlabel('时间 (s)');
    ylabel('线速度 (m/s)');
    title('线速度曲线', 'FontSize', 11, 'FontWeight', 'bold');
    ylim([min(controls(1, :))-0.1, max(controls(1, :))+0.1]);
    
    % === 子图4: 角速度曲线 ===
    subplot(2, 2, 4);
    hold on;
    grid on;
    plot(time, controls(2, :), 'r-', 'LineWidth', 1.5);
    xlabel('时间 (s)');
    ylabel('角速度 (rad/s)');
    title('角速度曲线', 'FontSize', 11, 'FontWeight', 'bold');
    
    % 添加统计信息
    sgtitle(sprintf('轨迹规划概览 | 总路程: %.1fm | 运行时间: %.1fs', ...
        calculate_trajectory_length(trajectory), time(end)), ...
        'FontSize', 13, 'FontWeight', 'bold');
    
    % 检查安全性
    collision_count = 0;
    for t = 1:size(trajectory, 2)
        [collision, ~] = check_collision(trajectory(:, t), obstacles, 0.5);
        if collision
            collision_count = collision_count + 1;
        end
    end
    
    if collision_count > 0
        warning_text = sprintf('警告: 检测到 %d 个潜在碰撞点!', collision_count);
        fprintf('\n%s\n', warning_text);
        annotation('textbox', [0.3, 0.01, 0.4, 0.05], 'String', warning_text, ...
            'HorizontalAlignment', 'center', 'FontSize', 11, 'Color', 'r', ...
            'FontWeight', 'bold', 'EdgeColor', 'none');
    else
        safe_text = '✓ 轨迹安全，未检测到碰撞';
        fprintf('\n%s\n', safe_text);
        annotation('textbox', [0.3, 0.01, 0.4, 0.05], 'String', safe_text, ...
            'HorizontalAlignment', 'center', 'FontSize', 11, 'Color', [0, 0.6, 0], ...
            'FontWeight', 'bold', 'EdgeColor', 'none');
    end
end

function length = calculate_trajectory_length(trajectory)
% 计算轨迹总长度
    length = 0;
    for i = 2:size(trajectory, 2)
        dx = trajectory(1, i) - trajectory(1, i-1);
        dy = trajectory(2, i) - trajectory(2, i-1);
        length = length + sqrt(dx^2 + dy^2);
    end
end

